/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "CTopoOccuMCL.h"
#include "JPEGEncoder.hpp"

#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch
#include "update_pose.h" 

#define NUM_OF_MCL_RUNS 7
#define MAP_FILE_NAME "../../res/autmap-koko.txt"

#define MAP_SCAN_FILE_NAME "../../res/autmap-scans.txt"



/// For reading the data from file

float *dist;
TOccupancyGrid *O;
int oNx,oNy;
vector<mcl::pose> godo;
vector<mcl::pose> odo;
TOccupancyGrid slamO(10.1,10.1,0.05,Grid::TCoord2D(0,0));
float *orig_map_data=NULL;



void initMapStuff(){
		O = new TOccupancyGrid(1,1,0.04,Grid::TCoord2D(0,0));
		
		if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
		slamO.setGridToValue(0.5);
		slamO.loadLineMap(MAP_FILE_NAME, 1.0,1.0);
		
		O->getDim(oNx, oNy);
		dist = (float *) malloc(oNx*oNy*sizeof(float));
		orig_map_data = (float *) malloc(oNx*oNy*sizeof(float));
		float *dataPtr = O->getDataPtr();
		memcpy(orig_map_data,dataPtr,oNx*oNy*sizeof(float));
		
		
		fprintf(stderr,"*** Computing distance transform ***\n");
		O->distanceTransform(dist, 1.0);	 
		fprintf(stderr,"Done!\n");
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}

int readMapData(FILE *f,mcl::scan &meas, mcl::pose &p){
		int i;
		char c;
		fscanf(f,"%f %f %f ", &p.x,&p.y,&p.a); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}



void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
				v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
				setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}
void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}
void readMCLfile(const char *fname, vector< vector<mcl::pose> >	&pose_storage){
	
		FILE *f = fopen(fname,"rt");
		if(f == NULL){
				fprintf(stderr,"readMCLfile:: Unable to open '%s'\n",fname);
				exit(1);
		}
	
		vector<mcl::pose> tmp;
		for(int i=0;i<NUM_OF_MCL_RUNS;i++){
				pose_storage.push_back(tmp);
		}
		
	
		mcl::pose p;
	
		while(!feof(f)){
		
				for(int i=0;i<NUM_OF_MCL_RUNS-1;i++){
						fscanf(f,"%f %f %f ",&p.x,&p.y,&p.a);
						pose_storage[i].push_back(p);
				}
				fscanf(f,"%f %f %f\n",&p.x,&p.y,&p.a);
				pose_storage[NUM_OF_MCL_RUNS-1].push_back(p);
		}
	
		fprintf(stderr,"We have %d poses in %d vectors\n",pose_storage[0].size(),pose_storage.size());
	
		fclose(f);
}		

int main(int argc, char *argv[]){
		int Nx,Ny;
		unsigned char *rgb,*rgb2;
		
		initMapStuff(); ///< For correlation
		
		///Initialize Image display
		int slamNx,slamNy;
		slamO.getDim(slamNx,slamNy);
		fprintf(stderr,"Image size = (%d,%d) and (%d,%d)\n",slamNx,slamNy,oNx,oNy);
		rgb = (unsigned char *) malloc(3*oNx*oNy);
		rgb2 = (unsigned char *) malloc(3*slamNx*slamNy);
		addImageWindow(slamNx, slamNy);
		glutImageGUI_Start();
		
		mcl::scan s; ///< for occupancy grid
		s.allocate(361);
		mcl::pose odo_cur, odo_ref,mest;
		Grid::TScan gscan;
		gscan.alloc(361);
		
		//displayImage(0, rgb2,slamNx,slamNy);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		
		///Laser odometry file
		vector<mcl::pose> orig_odo;
		vector<mcl::scan> scans;
		
		///Tuned path
		vector<mcl::pose> path_final;
		
		setRGBmap(rgb2,&slamO, 200,255, 200);
		fprintf(stderr,"Saving the original JPEG\n");
		saveJPEG("autmap-orig.jpg",rgb2,slamNx,slamNy);
		
		FILE *mapfile = fopen(MAP_SCAN_FILE_NAME,"rt");
		if(mapfile == NULL){
				fprintf(stderr,"No map file \n");
		}else{
				while(!feof(mapfile)){
						readMapData(mapfile,s,odo_cur);
						for(int j=0;j<s.N;j++){
								gscan.r[j] = s.r[j];
								if(gscan.r[j] > 8.0) gscan.r[j] = 0; 
								gscan.a[j] = s.a[j];
						}
						O->setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, gscan,0.3,0.9);
						slamO.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, gscan,0.3,0.9);
				}
				fclose(mapfile);
		}
		setRGBmap(rgb2,&slamO, 200,255, 200);
		fprintf(stderr,"Saving the JPEG\n");
		saveJPEG("autmap-matched.jpg",rgb2,slamNx,slamNy);
		/*
		while(1){
				setRGBmap(rgb2,&slamO, 200,255, 200);
				displayImage(0, rgb2,slamNy,slamNx);
				usleep(1000*1000);
		}*/

		return 0;
}
